using System;
using System.Drawing;

namespace Diversity.Robotics.Navigation
{
    public class ConeModel4Gaussians : IConeModel
    {
        private double _maxRange;
        private double _coneWidth;
        private double _obstacleSize;

        private double _emptyDistanceVariance;
        private double _emptyAngleVariance;

        private double _occDistanceVariance;
        private double _occAngleVariance;

        private double _kEmpty;
        private double _kOcc;

        private double _cutOff;

        private TrigUtils _trig;
       // private ProbabilityUtils ProbabilityUtils;

        public ConeModel4Gaussians(double maxRange, double coneWidth, double obstacleSize)
        {
            _maxRange = maxRange;
            _coneWidth = coneWidth;
            _obstacleSize = obstacleSize;

            _kEmpty = .49;
            _kOcc = .49;

            _cutOff = 2.5;
            _emptyAngleVariance = Math.Pow(_coneWidth / 6.0, 2);
            _occDistanceVariance = Math.Pow(_obstacleSize / 6, 2);
            _occAngleVariance = Math.Pow(_coneWidth / 6, 2);

            _trig = new TrigUtils();
        }

        public double MaxRange
        {
            get { return _maxRange; }
            set { _maxRange = value; }
        }

        public double ConeWidth
        {
            get { return _coneWidth; }
            set { _coneWidth = value; }
        }

        public double ObstacleSize
        {
            get { return _obstacleSize; }
            set { _obstacleSize = value; }
        }

        public double[,] UpdateMap(double[,] map, Pose2D state, double[] reading, double cmPerPixel,
                                   out Rectangle updateRect)
        {
            throw new NotImplementedException();
        }

        public void UpdateMap(ref double[,] map, DistanceReading reading, double cmPerPixel)
        {
            double readingTheta = _trig.CleanAngle(reading.Theta);
            double halfConeWidth = _coneWidth / 2;
            int mapWidth = map.GetLength(0);
            int mapHeight = map.GetLength(1);
            int rX = (int)Math.Round(reading.X / cmPerPixel);
            int rY = (int)Math.Round(reading.Y / cmPerPixel);
            int scaledDistance = (int)Math.Round(reading.Distance / cmPerPixel);
            int scaledMaxDistance = (int)Math.Round((_maxRange) / cmPerPixel) - 1;

            double halfScaledObstacleSize = _obstacleSize / (2 * cmPerPixel);
            double modelRange = scaledDistance + halfScaledObstacleSize;

            // find the box containing this beam
            int leftX = (int)Math.Round(rX + Math.Cos(readingTheta - halfConeWidth) * modelRange);
            int leftY = (int)Math.Round(rY + Math.Sin(readingTheta - halfConeWidth) * modelRange);
            int rightX = (int)Math.Round(rX + Math.Cos(readingTheta + halfConeWidth) * modelRange);
            int rightY = (int)Math.Round(rY + Math.Sin(readingTheta + halfConeWidth) * modelRange);
            int centerX = (int)Math.Round(rX + Math.Cos(readingTheta) * modelRange);
            int centerY = (int)Math.Round(rY + Math.Sin(readingTheta) * modelRange);
            int minBoxX = Math.Max(0, (Math.Min(Math.Min(Math.Min(rX, leftX), rightX), centerX)));
            int minBoxY = Math.Max(0, (Math.Min(Math.Min(Math.Min(rY, leftY), rightY), centerY)));
            int maxBoxX = Math.Min(mapWidth, Math.Max(Math.Max(Math.Max(rX, leftX), rightX), centerX));
            int maxBoxY = Math.Min(mapHeight, Math.Max(Math.Max(Math.Max(rY, leftY), rightY), centerY));
            // Console.Out.WriteLine("Box: " + minBoxX + "," + minBoxY + " " + maxBoxX + "," + maxBoxY);
            double maxTest = Math.Min(scaledMaxDistance - halfScaledObstacleSize, scaledDistance + halfScaledObstacleSize);
            //  updateRect = new Rectangle(minBoxX, minBoxY, maxBoxX - minBoxX, maxBoxY - minBoxY);
            // update the cells


            _emptyDistanceVariance = Math.Pow((scaledDistance - _obstacleSize / 2) * .45, 2);

            for (int x = minBoxX; x < maxBoxX; x++)
            {
                for (int y = minBoxY; y < maxBoxY; y++)
                {
                    double r = Math.Sqrt(Math.Pow(rX - x, 2) + Math.Pow(rY - y, 2));
                    if (r <= maxTest)
                    {
                        double fi = Math.Atan2(y - rY, x - rX);
                        double dTheta = Math.Abs(_trig.GetDifference(readingTheta, fi));

                        if (dTheta <= halfConeWidth)
                        {
                            // prob is defined as
                            double change =0;
                            if (r < scaledMaxDistance - halfScaledObstacleSize && Math.Abs(r - scaledDistance) < halfScaledObstacleSize)
                            {
                                if (r < scaledDistance)
                                {
                                    // mix the models
                                   
                                    // empty
                                    double distanceComponent = -Math.Pow(r -scaledDistance/3.0, 2) / _emptyDistanceVariance;
                                    double angleComponent = -Math.Pow(dTheta , 2) / _emptyAngleVariance;
                                    change = .5 - _kEmpty * Math.Pow(Math.E, distanceComponent + angleComponent);

                                    // occupied
                                    double occDistanceComponent = -Math.Pow(r - scaledDistance, 2) / _occDistanceVariance;
                                    double occAngleComponent = -Math.Pow(dTheta , 2) / _occAngleVariance;
                                    change += _kOcc * Math.Pow(Math.E, occDistanceComponent + occAngleComponent);
                           
                                }
                                else
                                {
                                    // occupied 2d gaussian
                                    double occDistanceComponent = -Math.Pow(r - scaledDistance, 2) / _occDistanceVariance;
                                    double occAngleComponent = -Math.Pow(dTheta , 2) / _occAngleVariance;
                                    change += 0.5 + _kOcc * Math.Pow(Math.E, occDistanceComponent + occAngleComponent);
                                }
                            }
                            else
                            {
                                // empty 2d gaussian
                                double distanceComponent = -Math.Pow(r - (scaledDistance - _obstacleSize / 2) / 3.0, 2) / _emptyDistanceVariance;
                                double angleComponent = -Math.Pow(dTheta , 2) / _emptyAngleVariance;
                                change = .5 -_kEmpty * Math.Pow(Math.E, distanceComponent + angleComponent);
                            }
                           // if (Math.Log(change/(1 - change)) < 0)
                           // {
                            //    Console.Out.WriteLine("Decreased by " + Math.Log(change / (1 - change)));
                            //}
                            map[x, y] += Math.Log(change/(1 - change));
                            if (map[x,y] > _cutOff)
                            {
                                map[x, y] = _cutOff;
                            } else if (map[x,y] < -_cutOff)
                            {
                                map[x, y] = - _cutOff;
                            }
                        }
                    }
                }
            }
           // return map;
        }

        // same but without the ref
        public double[,] UpdateMap( double[,] map, DistanceReading reading, double cmPerPixel)
        {
            double readingTheta = _trig.CleanAngle(reading.Theta);
            double halfConeWidth = _coneWidth / 2;
            int mapWidth = map.GetLength(0);
            int mapHeight = map.GetLength(1);
            int rX = (int)Math.Round(reading.X / cmPerPixel);
            int rY = (int)Math.Round(reading.Y / cmPerPixel);
            int scaledDistance = (int)Math.Round(reading.Distance / cmPerPixel);
            int scaledMaxDistance = (int)Math.Round((_maxRange) / cmPerPixel) - 1;

            double halfScaledObstacleSize = _obstacleSize / (2 * cmPerPixel);
            double modelRange = scaledDistance + halfScaledObstacleSize;

            // find the box containing this beam
            int leftX = (int)Math.Round(rX + Math.Cos(readingTheta - halfConeWidth) * modelRange);
            int leftY = (int)Math.Round(rY + Math.Sin(readingTheta - halfConeWidth) * modelRange);
            int rightX = (int)Math.Round(rX + Math.Cos(readingTheta + halfConeWidth) * modelRange);
            int rightY = (int)Math.Round(rY + Math.Sin(readingTheta + halfConeWidth) * modelRange);
            int centerX = (int)Math.Round(rX + Math.Cos(readingTheta) * modelRange);
            int centerY = (int)Math.Round(rY + Math.Sin(readingTheta) * modelRange);
            int minBoxX = Math.Max(0, (Math.Min(Math.Min(Math.Min(rX, leftX), rightX), centerX)));
            int minBoxY = Math.Max(0, (Math.Min(Math.Min(Math.Min(rY, leftY), rightY), centerY)));
            int maxBoxX = Math.Min(mapWidth, Math.Max(Math.Max(Math.Max(rX, leftX), rightX), centerX));
            int maxBoxY = Math.Min(mapHeight, Math.Max(Math.Max(Math.Max(rY, leftY), rightY), centerY));
            // Console.Out.WriteLine("Box: " + minBoxX + "," + minBoxY + " " + maxBoxX + "," + maxBoxY);
            double maxTest = Math.Min(scaledMaxDistance - halfScaledObstacleSize, scaledDistance + halfScaledObstacleSize);
            //  updateRect = new Rectangle(minBoxX, minBoxY, maxBoxX - minBoxX, maxBoxY - minBoxY);
            // update the cells


            _emptyDistanceVariance = Math.Pow((scaledDistance - _obstacleSize / 2) * .45, 2);

            for (int x = minBoxX; x < maxBoxX; x++)
            {
                for (int y = minBoxY; y < maxBoxY; y++)
                {
                    double r = Math.Sqrt(Math.Pow(rX - x, 2) + Math.Pow(rY - y, 2));
                    if (r <= maxTest)
                    {
                        double fi = Math.Atan2(y - rY, x - rX);
                        double dTheta = Math.Abs(_trig.GetDifference(readingTheta, fi));

                        if (dTheta <= halfConeWidth)
                        {
                            // prob is defined as
                            double change = 0;
                            if (r < scaledMaxDistance - halfScaledObstacleSize && Math.Abs(r - scaledDistance) < halfScaledObstacleSize)
                            {
                                if (r < scaledDistance)
                                {
                                    // mix the models

                                    // empty
                                    double distanceComponent = -Math.Pow(r - scaledDistance / 3.0, 2) / _emptyDistanceVariance;
                                    double angleComponent = -Math.Pow(dTheta, 2) / _emptyAngleVariance;
                                    change = .5 - _kEmpty * Math.Pow(Math.E, distanceComponent + angleComponent);

                                    // occupied
                                    double occDistanceComponent = -Math.Pow(r - scaledDistance, 2) / _occDistanceVariance;
                                    double occAngleComponent = -Math.Pow(dTheta, 2) / _occAngleVariance;
                                    change += _kOcc * Math.Pow(Math.E, occDistanceComponent + occAngleComponent);

                                }
                                else
                                {
                                    // occupied 2d gaussian
                                    double occDistanceComponent = -Math.Pow(r - scaledDistance, 2) / _occDistanceVariance;
                                    double occAngleComponent = -Math.Pow(dTheta, 2) / _occAngleVariance;
                                    change += 0.5 + _kOcc * Math.Pow(Math.E, occDistanceComponent + occAngleComponent);
                                }
                            }
                            else
                            {
                                // empty 2d gaussian
                                double distanceComponent = -Math.Pow(r - (scaledDistance - _obstacleSize / 2) / 3.0, 2) / _emptyDistanceVariance;
                                double angleComponent = -Math.Pow(dTheta, 2) / _emptyAngleVariance;
                                change = .5 - _kEmpty * Math.Pow(Math.E, distanceComponent + angleComponent);
                            }
                            // if (Math.Log(change/(1 - change)) < 0)
                            // {
                            //    Console.Out.WriteLine("Decreased by " + Math.Log(change / (1 - change)));
                            //}
                            map[x, y] += Math.Log(change / (1 - change));
                            if (map[x, y] > _cutOff)
                            {
                                map[x, y] = _cutOff;
                            }
                            else if (map[x, y] < -_cutOff)
                            {
                                map[x, y] = -_cutOff;
                            }
                        }
                    }
                }
            }
             return map;
        }
    }
}
